#include "mecanum_chassis.h"
#include <math.h>

// 四麦轮电机定义 - 全部使用3508电机
extern  Can_motor   m_0x201_can1;       //左前轮3508
extern  Can_motor   m_0x202_can1;       //右前轮3508  
extern  Can_motor   m_0x203_can1;       //左后轮3508
extern  Can_motor   m_0x204_can1;       //右后轮3508

static Can_motor* motor_pointer[4];  // 麦轮电机指针数组
osThreadId_t	ContrlerTaskHandle;		    
osThreadAttr_t ContrlerTask_attributes;
remote_control_t remote_control;
mecanum_control_t mecanum_control;
road_plan_t road_plan;    //路径规划

//圆周率
#define PI  3.14159265
// 麦轮参数定义
#define WHEEL_RADIUS          0.01f    // 轮子半径 (米)
#define WHEEL_BASE_LENGTH     0.01f      // 前后轮距 (米)
#define WHEEL_BASE_WIDTH      0.01f      // 左右轮距 (米)
#define MAX_WHEEL_SPEED       1000.0f   // 最大轮速 (rpm)
/*** 
 * @description: 控制器任务函数
 * @param {void} *argument
 * @return {*}
 */
extern "C" void ControlerTask(void *argument)  // 确保有 extern "C"
{

}

void remote_cmd_parser(remote_control_t* _remote_control);

/*** 
 * @description: 设置控制器任务
 * @return {*}
 */
extern "C" void controler_task_init()
{
    ContrlerTask_attributes.name = "ContrlerTask";
    ContrlerTask_attributes.stack_size = 128 * 4;
    ContrlerTask_attributes.priority = (osPriority_t) osPriorityBelowNormal7;   
    ContrlerTaskHandle = osThreadNew(ControlerTask, NULL, &ContrlerTask_attributes);
    
    // 初始化
    motor_pointer[0] = &m_0x201_can1;  // 左前轮
    motor_pointer[1] = &m_0x202_can1;  // 右前轮
    motor_pointer[2] = &m_0x203_can1;  // 左后轮  
    motor_pointer[3] = &m_0x204_can1;  // 右后轮
}

/*** 
 * @description: 控制器任务函数
 * @param {void} *argument
 * @return {*}
 */


/*** 
 * @description: 麦轮运动学计算
 * @param {float} vx X方向速度 (-10 ~ +10)
 * @param {float} vy Y方向速度 (-10 ~ +10)  
 * @param {float} vz 旋转速度 (-10 ~ +10)
 * @param {float*} wheel_speeds 输出的四个轮子速度数组 (rpm)
 * @return {*}
 */
void mecanum_kinematics_calculate(float vx, float vy, float vz, float* wheel_speeds)
{
    // 麦轮运动学公式
    // 左前轮: V1 = vx - vy - (Lx + Ly) * vz
    // 右前轮: V2 = vx + vy + (Lx + Ly) * vz  
    // 左后轮: V3 = vx + vy - (Lx + Ly) * vz
    // 右后轮: V4 = vx - vy + (Lx + Ly) * vz
    //在左右坡做决策只需要左右取反
	
    float L_sum = WHEEL_BASE_LENGTH + WHEEL_BASE_WIDTH;
    
    wheel_speeds[0] = vx - vy - L_sum * vz;  // 左前轮
    wheel_speeds[1] = vx + vy + L_sum * vz;  // 右前轮
    wheel_speeds[2] = vx + vy - L_sum * vz;  // 左后轮
    wheel_speeds[3] = vx - vy + L_sum * vz;  // 右后轮
    

    for(int i = 0; i < 4; i++) {
        wheel_speeds[i] = wheel_speeds[i] * (MAX_WHEEL_SPEED / 10.0f);
    }
    
    float max_speed = 0;
    for(int i = 0; i < 4; i++) {
        if(fabs(wheel_speeds[i]) > max_speed) {
            max_speed = fabs(wheel_speeds[i]);
        }
    }
    
    if(max_speed > MAX_WHEEL_SPEED) {
        for(int i = 0; i < 4; i++) {
            wheel_speeds[i] = (wheel_speeds[i] / max_speed) * MAX_WHEEL_SPEED;
        }
    }
}

/*** 
 * @description: 麦轮移动控制
 * @param {float} vx X方向速度 (-10 ~ +10)
 * @param {float} vy Y方向速度 (-10 ~ +10)
 * @param {float} vz 旋转速度 (-10 ~ +10)
 * @return {*}
 */
void car_mecanum_move(float vx, float vy, float vz)
{
    float wheel_speeds[4];
    
    mecanum_kinematics_calculate(vx, vy, vz, wheel_speeds);
    
    for(int i = 0; i < 4; i++) {
        motor_pointer[i]->set_speed(wheel_speeds[i]);
    }
}

/*** 
 * @description: 车辆停止
 * @return {*}
 */
void car_stop()
{
    // 停止所有四个麦轮电机
    for(int i = 0; i < 4; i++) {
        motor_pointer[i]->set_stop();
    }
}

/*** 
 * @description: 车辆运行准备
 * @return {*}
 */
void car_run()
{
    // 设置所有四个麦轮电机为运行状态
    // 使用measure_config配置电机为运行状态和无角度环模式
    for(int i = 0; i < 4; i++) {
        motor_pointer[i]->measure_config(Can_motor::MOTOR_STATE::RUN,
                                       Can_motor::MOTOR_STATE::ANGLE_NO,
                                       0x201 + i);
    }
}


// 向前移动car_mecanum_move(1.0f, 0.0f, 0.0f);
// 向后移动car_mecanum_move(-1.0f, 0.0f, 0.0f);
//向左平移car_mecanum_move(0.0f, 1.0f, 0.0f);
// 向右平移  car_mecanum_move(0.0f, -1.0f, 0.0f);
// 原地顺时针旋转car_mecanum_move(0.0f, 0.0f, 5.0f);
// 原地逆时针旋转car_mecanum_move(0.0f, 0.0f, -5.0f);

/*** 
 * @description: 路径规划
 * @return {*}
 */
void moveto(float speed , float x , float y)
{
	float dx = x - road_plan.position_x;
	float dy = y - road_plan.position_y;
	float distance = sqrt(pow(dx, 2) + pow(dy, 2));
	float angle = atan(dy / dx);
	if (dx < 0 && dy > 0) angle += PI;
    if (dx < 0 && dy < 0) angle += PI;
    if (dx > 0 && dy < 0) angle += 2 * PI;
	float det = angle - road_plan.radian;
	if(det > 0)
	{
		car_mecanum_move(0.0f, 0.0f, -5.0f);   //逆时针旋转
	}
	else if(det<0)
	{
		car_mecanum_move(0.0f, 0.0f, 5.0f);    //顺时针旋转
	}
	car_mecanum_move(1.0f, 0.0f, 0.0f);
	//将达到后的位置定为当前位置
	road_plan.position_x = x;
	road_plan.position_y = y;
	road_plan.radian = angle;
}

/*** 
 * @description: 修正位置
 * @return {*}
 */
void position_correction()
{
	//具体坐标后续会根据实际情况修改
	float points[] = {
		7.35478, 1.56377,    //点(x0,y0)
		13.6229, 6.07313,    //点(x1,y1)，以此类推
		17.833, 13.0008, 
		19.2331, 21.5279, 
		17.3726, 30.6274, 
		12.1537, 39.1698, 
		3.84847, 46.0391, 
		-6.9224, 50.2492, 
		-19.2453, 51.0482,
		-32, 48,
		-43.9667,41.0364, 
		-53.9452, 30.4721, 
		-60.8729, 16.9837, 
		-63.9308, 1.55155, 
		-62.6274, -14.6274, 
		-56.8514, -30.2492, 
		-46.8884, -44.0236, 
		-33.4, -54.7945, 
		-17.3666, -61.6483,
		-7.3666, -63.6483, 
	};
	int pointsCnt = 20;
	
	//初始化机器人位置
	road_plan.position_x = points[0];
	road_plan.position_y = points[1];
	road_plan.radian = 0;
	
	//修正位置
    for (int i = 1; i < pointsCnt; i++) 
	{
        points[i * 2] *= 0.7;
        points[i * 2 + 1] *= 0.7;
    }
    for (int i = 3; i < pointsCnt; i++) 
	{
        points[i * 2] *= 1.1;
        points[i * 2 + 1] *= 1.1;
    }
    for (int i = 5; i < pointsCnt; i++) 
	{
        points[i * 2] *= 1.1;
        points[i * 2 + 1] *= 1.1;
    }
    for (int i = 8; i < pointsCnt; i++) 
	{
        points[i * 2] *= 1.1;
        points[i * 2 + 1] *= 1.1;
    }
    for (int i = 13; i < pointsCnt; i++) 
	{
        points[i * 2] *= 1.05;
        points[i * 2 + 1] *= 1.05;
    }
    for (int i = 16; i < pointsCnt; i++) 
	{
        points[i * 2] *= 0.99;
        points[i * 2 + 1] *= 0.99;
    }

	
    for (int i = 1; i < pointsCnt; ++i) 
	{
        moveto(1, points[i * 2], points[i * 2 + 1]);
        HAL_Delay(20);
    }
	
	car_stop();
}
